#pragma once
#include "SC_ESKF/include/eskf.h"
#include <vector>
#include "utils/geographic_easy_use.hpp"
#include <yaml-cpp/yaml.h>

/// @brief 用于进行一次ESKF的数据组
struct MeasureGroupType{
    /// @brief t->t+1时间内的k个imu数据，t为GNSS时间戳
    std::vector<SlamCraft::IMU> imus_in;
    /// @brief t+1时刻的gnss数据
    SlamCraft::GNSSDataType end_gnss;
    void clear(){
        imus_in.clear();
    }
};
class GNSS_IMU_Align
{
private:
    SlamCraft::ESKF eskf;

    Eigen::Matrix3d Offset_R_L_I  ;
    Eigen::Vector3d Offset_T_L_I;
    Eigen::Vector3d cov_gyr;
    Eigen::Vector3d cov_acc;
    Eigen::Vector3d cov_bias_gyr;
    Eigen::Vector3d cov_bias_acc;
    SlamCraft::IMU last_imu;
    SlamCraft::GNSSDataType now_gnss;
    SlamCraft::EZGeographic ez_geographic;


public:
    GNSS_IMU_Align(/* args */);
    ~GNSS_IMU_Align();
    void process_group( MeasureGroupType&measure_group);
    bool init(const MeasureGroupType&measure_group);
    SlamCraft::ESKF::State_18 get_x();
    void calc_eskf_zhr(const SlamCraft::ESKF::State_18& X,Eigen::MatrixXd & Z,Eigen::MatrixXd & H,Eigen::MatrixXd & R);
    Eigen::Vector3d debug_for_gnss_positon(const SlamCraft::GNSSDataType&gnss);
    void save_init_pose(const SlamCraft::GNSSDataType&gnss);
    SlamCraft::EZGeographic&get_geo_cvt(){
        return ez_geographic;
    }
};

